
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_motor.h
  * @author     baiyang
  * @date       2022-11-28
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sitl_input.h"

#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum sim_servo_type {
    SERVO_NORMAL,
    SERVO_RETRACT
};

typedef struct sim_motor* sim_motor_t;
typedef const struct sim_motor* sim_motor_const_t;

struct sim_motor {
    float angle;
    float yaw_factor;
    uint8_t servo;
    uint8_t display_order;

    // support for tilting motors
    int8_t roll_servo;
    float roll_min, roll_max;
    int8_t pitch_servo;
    float pitch_min, pitch_max;

    // support for servo slew rate
    enum sim_servo_type servo_type;
    float servo_rate; // seconds per 60 degrees
    uint64_t last_change_usec;
    float last_roll_value, last_pitch_value;

    float mot_pwm_min;
    float mot_pwm_max;
    float mot_spin_min;
    float mot_spin_max;
    float mot_expo;
    float slew_max;
    float current;
    float power_factor;
    float voltage_max;
    float effective_prop_area;
    float max_outflow_velocity;
    float true_prop_area;
    float momentum_drag_coefficient;
    float diagonal_size;

    float last_command;
    uint64_t last_calc_us;

    Vector3f_t position;
    Vector3f_t thrust_vector;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_motor_ctor(sim_motor_t motor, uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order);
void sim_motor_ctor2(sim_motor_t motor, uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order,
          int8_t _roll_servo, float _roll_min, float _roll_max,
          int8_t _pitch_servo, float _pitch_min, float _pitch_max);

void sim_motor_init(sim_motor_t motor);

void sim_motor_calculate_forces(sim_motor_t motor, const struct sitl_input *input,
          uint8_t motor_offset,
          Vector3f_t *torque,
          Vector3f_t *thrust,
          const Vector3f_t *velocity_air_bf,
          const Vector3f_t *gyro,
          float air_density,
          float voltage,
          bool use_drag);

uint16_t sim_motor_update_servo(sim_motor_const_t motor, uint16_t demand, uint64_t time_usec, float *last_value);

void sim_motor_setup_params(sim_motor_t motor, uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
          float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
          float _velocity_max, const Vector3f_t* _position, const Vector3f_t* _thrust_vector, float _yaw_factor,
          float _true_prop_area, float _momentum_drag_coefficient);

float sim_motor_pwm_to_command(sim_motor_const_t motor, float pwm);
float sim_motor_calc_thrust(sim_motor_const_t motor, float command, float air_density, float velocity_in, float voltage_scale);

// calculate current and voltage
static inline float sim_motor_get_current(sim_motor_const_t motor) { return motor->current; }

// override slew limit
static inline void sim_motor_set_slew_max(sim_motor_t motor, float _slew_max) { motor->slew_max = _slew_max; }
static inline float sim_motor_get_command(sim_motor_const_t motor) { return motor->last_command;} 
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



